#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "robot_config.h"
//test variables
unsigned long _millis = 0;
int _a0l = -99;
int _a1l = -99;
int _a0r = -99;
int _a1r = -99;
int _corrected_motor_left = -99;
int _corrected_motor_right = -99;
#define TIMEHOPS 100 //ms



//move.h
int motor_left_dir = 1; //direction for motor expected to go. 1 means forward
int motor_right_dir = 1;
double req_motor_left = 0; //store required wheel velocity
double req_motor_right = 0;
double old_motor_left = 0; //store to compare if speed changed since last cmd_vel
double old_motor_right = 0;
long odom_ticks_left_since_cmd = 0;
long odom_ticks_right_since_cmd = 0;
unsigned long millis_since_cmd = 0;

int motor_vel_to_pwm(double vel) {
	return vel / MAX_VEL * MOTOR_MAX;
}

int is_equal_3decplaces(double a, double b) {
		long long ai = a * 1000;
		long long bi = b * 1000;
		return ai == bi;
}

#define HIGH 0x1
#define LOW  0x0

#define min(a,b) ((a)<(b)?(a):(b))
#define max(a,b) ((a)>(b)?(a):(b))
#define abs(x) ((x)>0?(x):-(x))
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
template <typename type>
type signNZ(type value) {
	return (value < 0) ? -1 : 1;
}

void digitalWrite(uint8_t pin, uint8_t val) {
  switch (pin) {
    case PIN_MOTOR_LEFT_0 :
      _a0l = val;
      break;
    case PIN_MOTOR_LEFT_1 :
      _a1l = val;
      break;
    case PIN_MOTOR_RIGHT_0 :
      _a0r = val;
      break;
    case PIN_MOTOR_RIGHT_1 :
      _a1r = val;
      break;
  }
}
void analogWrite(uint8_t pin, int val){
  switch (pin) {
    case PIN_MOTOR_LEFT_ENA :
    _corrected_motor_left = val;
    break;
  case PIN_MOTOR_RIGHT_ENA :
    _corrected_motor_right = val;
    break;
  }
}

void intr_left_encoder() {
  double bias = 1;
  if (_corrected_motor_left > 30 && _corrected_motor_left < 40)
    bias = (double)(_corrected_motor_left - 30) / 10.0;
  if (_corrected_motor_left < 30)
    bias = 0;
  double m_per_sec = (double)_corrected_motor_left * bias / MOTOR_MAX * MAX_VEL;
  double ticks_per_sec = m_per_sec * 1000 / TICK_LENGTH_MM;
  odom_ticks_left_since_cmd += ticks_per_sec / (1000 / TIMEHOPS);
}

void intr_right_encoder(){
  double bias = 1;
  if (_corrected_motor_right > 30 && _corrected_motor_right < 40)
    bias = (double)(_corrected_motor_right - 30) / 10.0;
  if (_corrected_motor_right < 30)
    bias = 0;
  double m_per_sec = (double)_corrected_motor_right * bias / MOTOR_MAX * MAX_VEL;
  double ticks_per_sec = m_per_sec * 1000 / TICK_LENGTH_MM;
  odom_ticks_right_since_cmd += ticks_per_sec / (1000 / TIMEHOPS);
}

unsigned long millis() {
  return _millis;
}

//input is global variable req_motor_left and req_motor_right, [meter/sec], max speed is normalized
void velocity_control_LR() {

	if ( (!is_equal_3decplaces(req_motor_left, old_motor_left)) || (!is_equal_3decplaces(req_motor_right, old_motor_right)) )  { //velocity command has changed
		//remove correction, use only estimated speeds for new velocity command
		old_motor_left = req_motor_left;
		old_motor_right = req_motor_right;
		millis_since_cmd = millis();
	}

	//number of ticks that that robot is behind. Can be also < 0 which means the root is ahead of required
	long correction_motor_left  = long((double)(millis() - millis_since_cmd) * req_motor_left  / TICK_LENGTH_MM) - odom_ticks_left_since_cmd;
	long correction_motor_right = long((double)(millis() - millis_since_cmd) * req_motor_right / TICK_LENGTH_MM) - odom_ticks_right_since_cmd;

	//include correction <0 - ow; MOTOR_MAX + ow> where ow is an overflow
	int corrected_motor_left  = motor_vel_to_pwm(abs(req_motor_left))  + constrain(correction_motor_left  * 20, 0, MOTOR_MAX);
	int corrected_motor_right = motor_vel_to_pwm(abs(req_motor_right)) + constrain(correction_motor_right * 20, 0, MOTOR_MAX);

	//remap if speed of any wheel overflows 0 or MOTOR_MAX
	if (corrected_motor_left < MOTOR_MIN)
		corrected_motor_left = 0;
	if (corrected_motor_right < MOTOR_MIN)
		corrected_motor_right = 0;

	int ow_left = 0;
	int ow_right = 0;
	int ow = 0;
	if (corrected_motor_left > MOTOR_MAX)
		ow_left = corrected_motor_left - MOTOR_MAX;
	if (corrected_motor_left > MOTOR_MAX)
		ow_right = corrected_motor_left - MOTOR_MAX;

	//subtract common overflow for both wheels
	ow = max(ow_left, ow_right);
	corrected_motor_left -= ow;
	corrected_motor_right -= ow;

	//derive direction from original speed
	//left
	int a0 = LOW;  //stop implicitly
	int a1 = LOW;
	if (req_motor_left < 0) {
		a0 = HIGH;
		motor_left_dir = -1;
	}
	if (req_motor_left > 0) {
		a1 = HIGH;
		motor_left_dir = 1;
	}
	digitalWrite(PIN_MOTOR_LEFT_0, a0);
	digitalWrite(PIN_MOTOR_LEFT_1, a1);
	analogWrite(PIN_MOTOR_LEFT_ENA, constrain(corrected_motor_left, 0, 255));

	//right
	a0 = LOW;
	a1 = LOW;
	if (req_motor_right < 0) {
		a0 = HIGH;
		motor_right_dir = -1;
	}
	if (req_motor_right > 0) {
		a1 = HIGH;
		motor_right_dir = 1;
	}
	digitalWrite(PIN_MOTOR_RIGHT_0, a0);
	digitalWrite(PIN_MOTOR_RIGHT_1, a1);
	analogWrite(PIN_MOTOR_RIGHT_ENA, constrain(corrected_motor_right, 0, 255));

}

// vel: linear velocity [m/sec]
// theta: angular velocity [rad/sec]
void velocity_control_VelTh(float vel, float th) {

	//limit to standard max values
	vel = constrain(vel, -MAX_VEL, MAX_VEL);
	th = constrain(th, -MAX_TH, MAX_TH);

	//combine speed and rotation and set as required speed for boot wheels
   //TODO replace by (http://www.openrobots.org/morse/doc/1.2/user/actuators/v_omega_diff_drive.html):
   //req_motor_left  = (vel - (AXIAL_LENGTH_MM / 2) * th) / (WHEEL_DIA / 2);
   //req_motor_right = (vel + (AXIAL_LENGTH_MM / 2) * th) / (WHEEL_DIA / 2);

	req_motor_left = vel - signNZ(vel) * th / (MAX_TH * MAX_VEL);
	req_motor_right = vel + signNZ(vel) * th / (MAX_TH * MAX_VEL);
	velocity_control_LR();

}


int main() {

  for (int i = 0; i < 100; i++) {

    if (i == 3) velocity_control_VelTh(0.05, 0.0);

    velocity_control_LR();
    intr_left_encoder();
    intr_right_encoder();
    printf("%lu::\t\t%d%dL%d %d%dR%d\n", _millis, _a0l, _a1l, _corrected_motor_left, _a0r, _a1r, _corrected_motor_right);
    _millis += TIMEHOPS; //wait 100ms
  }

  return 0;
}
